#ifndef __APP_MOTION_H__
#define __APP_MOTION_H__

#include "main.h"
#include "../Components/basic/basic.h"
#include "../Components/algorithm/algorithm.h"
// #include "basic.h"

#ifdef __cplusplus
#include <vector>
#include <map>
extern "C"
{
    class Motor
    {
    private:
        bool isInv;
        pwm_t pwm;
        gpio_t pinA, pinB, enc_pinA, enc_pinB;
        IRQn_Type enc_irq;

        float last_valid_speed=0;

        uint32_t last_intrupt_us = 0;
        uint32_t intrupt_us = 0;
        int8_t direction = 0;

        float target_speed = 0;
        pidController *pid = new pidController(1.5, 0.9, 0.05, 0.8);

        static std::map<uint16_t, Motor *> pin_to_motor_map;

        static void dispatch_interrupt(uint16_t GPIO_Pin);

        void handle_enc_a_interrupt();

        friend void user_motor_gpio_inturrupt_handler(uint16_t GPIO_Pin);

    public:
        typedef enum
        {
            MODE_FORWARD = 0,
            MODE_BACKWARD,
            MODE_NETURAL,
            MODE_STOP,
        } Motor_Mode;

        void set_torque(float torque);

        Motor(pwm_t pwm, gpio_t pinA, gpio_t pinB, gpio_t enc_pinA, gpio_t enc_pinB, IRQn_Type enc_irq, bool isInv = false);
        ~Motor();
        void set_moode(Motor_Mode mode);
        float get_speed();
        void set_target_speed(float speed) { this->target_speed = speed; };
        void poll(float dt = 0.05);
    };
}
#endif

#endif
